// mdvctrl - A video-driven controller for MD-25 motor drivers
// Copyright (C) 2010  Nuno J. Silva
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, version 3 of the License.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <iostream>

// blob library headers
#include "Blob.h"
#include "BlobResult.h"

#include "serial.h"
#include "strategy.h"

// Central position in a frame (which is 160 pixels wide)
#define POSITION_ZERO 80

// Base values for gain and speed
#define MIN_BASE_SPEED 165
#define FIXED_GAIN 0.2

// Maximum value for speed increase
#define SPEED_BOOST 45

// If this is defined, if the contoller is unable to see the track, it
// will use the previous steering (kind of zero order hold), instead
// of 0.
#define FALLBACK_USE_LAST_STEER

// Values used by blob filtering heuristics
#define MIN_BLOB_AREA 100
#define MAX_BLOB_AREA 8000

#define MIN_BLOB_MAXY  90
#define MAX_BLOB_MINY  90
#define MIN_BLOB_HEIGHT 30

// Keep some statistics
int zerosteers = 0;
int steers = 0;
int noblobs = 0;

// memorize the previous steering, so we can use it if we have no
// blob.
int previous_steer = POSITION_ZERO; 


double gain(int position) {
  double factor = abs(position - POSITION_ZERO);
  if (factor > 5){
    return FIXED_GAIN + 0.1;
  } else {
    return FIXED_GAIN * (factor/5.0) + 0.1;
  }
}

// Calculate the speed difference for the current displacement. The
// resulting value should be *added* to the left speed and
// *subtracted* from the right speed.
int calculate_difference(int position) {
  int relative_displacement = position - POSITION_ZERO;

  return (relative_displacement * gain(position));
}


// compute a speed factor, between 0 and 1, which will weight a speed
// bonus.
double get_speed_factor(int position) {
  double factor = abs(position - POSITION_ZERO);

  if (factor > 15){
    // If the object is far away, proceed with caution!
    return 0;
  } else {
    // otherwise go faster, where "faster" is defined by how close the
    // object is.
    return (1 - (factor/15.0));
  }
}


// compute a *base speed* to use when moving the robot. Here "base"
// means "before steering" and not "before applying the speed factor".
int base_speed(int position) {
  return  MIN_BASE_SPEED + SPEED_BOOST * get_speed_factor(position);
}


// This the point where the image manipulation ends and the mechanical
// control starts: given the "best blob", do some maths and steer the
// robot.
void steer_blob(int robotfd, int position) {
  steermove (robotfd, base_speed (position), calculate_difference (position) / 2.0);
}


// Given a frame in the format required by cvBlobsLib, calculate the
// position of the line in the frame.
int get_best_blob(IplImage * frame) {

  // build a blob mask, which marks the part of the frame we will
  // look at. ATM this is the whole frame.
  IplImage* mask = cvCreateImage (cvGetSize (frame), IPL_DEPTH_8U, 1);
  cvSet (mask, cvScalar (1));

  // The resulting choice. For now the function just returns "best"
  // (the mean coordinate in the X axis), but it might do something
  // fancier with the best blob in the future, so "best_blob" holds a
  // pointer to the CBlob structure.
  int best = 80;
  CBlob * best_blob = NULL;
  int best_area = 0;
      
  // Blob extraction: Get all blobs.
  CBlobResult  blobs = CBlobResult (frame, mask, 100, true);
  {
    int max = (&blobs)->GetNumBlobs ();
    int i;

    // Starts outputing the list of blobs
    std::cerr << "(";

    for (i = 0; i < max; i++) {
      CBlob * blob = (&blobs)->GetBlob (i);

      // Exclude "bad" blobs (our main criterion is area)
      //
      // cvblobslib has a function for this, but it seems to crash
      // when used here.
      //
      // we also check if the blob is tall enough and if it intersects
      // the lower and upper parts of the frame (the frame is "split"
      // in these parts using the line defined by MIN_BLOB_MAXY and
      // MAX_BLOB_MINY)
      if((blob->Area () > MIN_BLOB_AREA) &&
	 (blob->Area () < MAX_BLOB_AREA) &&
	 ((blob->MinY () < MAX_BLOB_MINY) ||
	  (blob->MaxY () > MIN_BLOB_MAXY) ||
	  ((blob->MaxY () - blob->MinY()) > MIN_BLOB_HEIGHT))) {

	// Get an estimate of the "central position" of the blob (the
	// "x" coordinate of its central point)
	CBlobGetXCenter getcenter = CBlobGetXCenter();
	int position = getcenter(blob);

	int area = blob->Area();

	// Output blob properties
	std::cerr << position << "/" << area << ", ";


	// Determinate if this is the best blob so far, using what GoF
	// would call a "strategy pattern".
	int (*heuristic)(int , int , int , int );

	// This uses the "furthest point" strategy, another one is the
	// "biggest blob" one (&biggest_blob). See "strategy.cpp".
	heuristic = &furthest_point;

	if ((best_blob == NULL) ||
	    ((*heuristic) (best, best_area, position, area))) {
	  best_blob = blob;
	  best_area = area;
	  best = position;	  
	}
	
      }
    }

    if (best_blob == NULL) {
#ifdef FALLBACK_USE_LAST_STEER
      best = previous_steer;
#else
      best=80;
#endif 
      noblobs++;
    }

    // end the list of blobs.
    std::cerr << ")" << std::endl;

    // Statistics
    if (best == 80) {
      ++zerosteers;
    } else {
      ++steers;
    }
  }

  // At this point the best blob was already picked, and its position
  // was recorded.

  // Save the choosen value to use next time if there's no blob.
  previous_steer = best;

  return best;
}


void print_stats (void) {
  std::cout << "Program ended. Some stats:" << std::endl
	    << "  Zero Steers: " << zerosteers << std::endl
	    << "  Valid Steers: " << steers << std::endl
	    << "  No blobs: " << noblobs << std::endl;
}
